// --------------------------------------------------------------------------------------------------------------------
// <copyright file="DifferentialDriveState.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Hardware.DAC
{
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Algorithms;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// The DifferentialDriveState class
    /// </summary>
    [DataContract]
    public class DifferentialDriveState : IDeviceState
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="DifferentialDriveState"/> class.
        /// </summary>
        public DifferentialDriveState()
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="DifferentialDriveState"/> class.
        /// </summary>
        /// <param name="other">The copy constructor param</param>
        public DifferentialDriveState(DifferentialDriveState other)
        {
            this.DeviceId = other.DeviceId;
            this.DeviceType = other.DeviceType;
            this.LeftWheelAccelerationInMetersPerSecondSquared = other.LeftWheelAccelerationInMetersPerSecondSquared;
            this.LeftWheelPositionInMeters = other.LeftWheelPositionInMeters;
            this.LeftWheelVelocityInMetersPerSecond = other.LeftWheelVelocityInMetersPerSecond;
            this.Model = other.Model;
            this.Pose = other.Pose;
            this.RightWheelAccelerationInMetersPerSecondSquared = other.RightWheelAccelerationInMetersPerSecondSquared;
            this.RightWheelPositionInMeters = other.RightWheelPositionInMeters;
            this.RightWheelVelocityInMetersPerSecond = other.RightWheelVelocityInMetersPerSecond;
            this.TimestampInMilliseconds = other.TimestampInMilliseconds;
            this.TimestampInTicks = other.TimestampInTicks;
            this.WheelBaseInMeters = other.WheelBaseInMeters;
        }

        /// <summary>
        /// Gets or sets the position of the left wheel in meters
        /// </summary> 
        [DataMember]
        public double LeftWheelPositionInMeters { get; set; }

        /// <summary>
        /// Gets or sets the velocity of the left wheel in meters/sec
        /// </summary>
        [DataMember]
        public double LeftWheelVelocityInMetersPerSecond { get; set; }

        /// <summary>
        /// Gets or sets the acceleration of the left wheel in meters/sec^2
        /// </summary>
        [DataMember]
        public double LeftWheelAccelerationInMetersPerSecondSquared { get; set; }

        /// <summary>
        /// Gets or sets the position of the right wheel in meters
        /// </summary>
        [DataMember]
        public double RightWheelPositionInMeters { get; set; }

        /// <summary>
        /// Gets or sets the velocity of the right wheel in meters/sec
        /// </summary>
        [DataMember]
        public double RightWheelVelocityInMetersPerSecond { get; set; }

        /// <summary>
        /// Gets or sets the acceleration of the right wheel in meters/sec^2
        /// </summary>
        [DataMember]
        public double RightWheelAccelerationInMetersPerSecondSquared { get; set; }

        /// <summary>
        /// Gets or sets the timestamp for the state.
        /// </summary>
        [DataMember]
        public long TimestampInTicks { get; set; }

        /// <summary>
        /// Gets or sets the timestamp in milliseconds (as opposed to hardware-dependent ticks).
        /// </summary>
        [DataMember]
        public long TimestampInMilliseconds { get; set; }

        /// <summary>
        /// Gets or sets the device identifier
        /// </summary>
        [DataMember]
        public int DeviceId { get; set; }

        /// <summary>
        /// Gets or sets the device model information
        /// </summary>
        [DataMember]
        public string Model { get; set; }

        /// <summary>
        /// Gets or sets the type of the device the sensor is related
        /// </summary>
        [DataMember]
        public DeviceType DeviceType { get; set; }

        /// <summary>
        /// Gets or sets the pose for the device/sensor
        /// </summary>
        [DataMember]
        public Pose Pose { get; set; }

        /// <summary>
        /// Gets or sets distance between the drive wheels in meters
        /// </summary>
        [DataMember]
        public double WheelBaseInMeters { get; set; }

        /// <summary>
        /// Interpolates the between the start and end states for the desiredTime.
        /// </summary>
        /// <param name="startTime">The start time for interpolation</param>
        /// <param name="endTime">The end time for interpolation</param>
        /// <param name="desiredTime">Desired time for interpolation. This must be between (inclusive) start and end times</param>
        /// <param name="start">Start value</param>
        /// <param name="end">End value</param>    
        /// <returns>The linear interpolated value at the desired time</returns>
        public static DifferentialDriveState Interpolate(long startTime, long endTime, long desiredTime, DifferentialDriveState start, DifferentialDriveState end)
        {
            DifferentialDriveState interpolated = new DifferentialDriveState(start);
            interpolated.LeftWheelPositionInMeters = Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.LeftWheelPositionInMeters, end.LeftWheelPositionInMeters);
            interpolated.RightWheelPositionInMeters = Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.RightWheelPositionInMeters, end.RightWheelPositionInMeters);
            interpolated.TimestampInMilliseconds = (long)Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.TimestampInMilliseconds, end.TimestampInMilliseconds);
            interpolated.TimestampInTicks = (long)Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.TimestampInTicks, end.TimestampInTicks);
            return interpolated;
        }
    }
}
